【Robotics】空间姿态表示法：旋转矩阵、欧拉角和四元数

本文介绍空间姿态的各种表示方法。

首先复习一下旋转矩阵： \[ { }_{\mathrm{B}}^{\mathrm{A}} \mathrm{R} = \left[\begin{array}{ccc} { }^{A}\vec{x}_{B} & { }^{A}\vec{y}_{B} & { }^{A}\vec{z}_{B} \end{array}\right] = \left[\begin{array}{c} { }^{B}\vec{x}_{A} { }^{T} \\ { }^{B}\vec{y}_{A} { }^{T} \\ { }^{B}\vec{z}_{A} { }^{T}\end{array}\right] \]

这个式子表示，有A、B两个坐标系，从B坐标到A坐标（ \(B\rightarrow A\) ）的旋转变换可以由这个 \(R\) 矩阵表示；其中，\(R\) 矩阵的每一列分别是B坐标的 \(x, y, z\) 轴在A坐标中的表示；而由于旋转矩阵的转置是它的逆，\(R\) 矩阵的每一行则分别是A坐标的 \(x, y, z\) 轴在B坐标中的表示。

例如下图，从B到A坐标系的旋转，我们可以直接把R写出来：

第一列，因为 \(X_B\) 轴与 \(Z_A\) 轴重合，所以是[0; 0; 1]；

第二列，\(Y_B\)轴与\(Y_A\)轴方向正好相反，所以是[0; -1; 0]；

同理，第三列是[1; 0; 0]。

如果按照每一行来写，则第一行是与ZB重合的XA轴[0，0，1]，第二行是与YB轴反方向的YA轴[0, -1, 0]，第三行则是与XB轴重合的ZA轴[1, 0, 0]。

例如，某个点绕Z轴旋转α角，也就是说旋转后的Z坐标是不变的，变化的只是X、Y坐标。因此甚至可以基于2D公式，不用推导的写出下面这个式子，因为为了满足相等关系，必须补充系数矩阵元素。 \[ \left(\begin{array}{c} x^{\prime} \\ y^{\prime} \\ z \end{array}\right)=\left(\begin{array}{ccc} \cos \alpha & -\sin \alpha & 0 \\ \sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \end{array}\right) \] 用旋转矩阵表示两个坐标系之间的旋转变换固然方便，但我们不禁要问，明明只是三个自由度的变换，为什么要用到九个数呢？假如你在操控一架飞机，要向左倾斜机身15度，拉升机头10度，是不是还要先把这样的指令转为旋转矩阵呢？在控制系统中，已知物体当前的姿态和理想的姿态，我们又要怎么计算姿态误差呢？

因为这种种的需求，我们需要了解和学习其他空间姿态表示方法。

欧拉角是欧拉引入用来描述刚体姿态的三个角，在所有空间姿态表示方法中，欧拉角可以说是最直观的一种。它用三个数描述从一个坐标系到另一个坐标系的变换，每个数分别是绕某一个坐标轴转动的角度。

欧拉角有静态和动态两种，

航空领域通常采用的旋转方式是ZYX顺序，下图是一架飞机按照ZYX组合进行旋转产生欧拉角的过程，其中，\(\psi\)为偏航角，\(\theta\) 为俯仰角，\(\phi\) 为滚转角。首先，我们绕机体NED系的Z轴旋转\(\psi\)角度，得到中间坐标系1，然后绕中间坐标系1的Y轴旋转\(\theta\)角得到中间坐标系2，之后再绕中间坐标系2的X轴旋转角度\(\phi\)就得到了机体轴坐标系，所有旋转都按照右手螺旋定则进行。

我们上面描述的三个旋转虽然在三维笛卡尔直角坐标系下进行，但都是平面旋转，他们的旋转矩阵分别是: \[ \begin{array}{l} R_{\psi}=\left[\begin{array}{ccc} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right] \\ R_{\theta}=\left[\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right] \\ R_{\phi}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \varphi & \sin \varphi \\ 0 & -\sin \varphi & \cos \varphi \end{array}\right] \end{array} \] 根据作用的顺序，则从机载NED坐标系到机体轴坐标系的欧拉转换矩阵是： \[ R_{b / m v}=R_{\phi} R_{\theta} R_{\psi}=\left[\begin{array}{ccc} \cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \sin \varphi \sin \theta \cos \psi-\cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi+\cos \varphi \cos \psi & \sin \varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi+\sin \varphi \sin \psi & \cos \varphi \sin \theta \sin \psi-\sin \varphi \cos \psi & \cos \varphi \cos \theta \end{array}\right] \] 只要\(\theta \neq \pm 90°\)，欧拉角可以描述清楚任何刚体的姿态以及角运动信息，而对于大部分飞行器来说，俯仰角也不会到90°，所以，使用欧拉角进行姿态控制完全可以满足使用要求。但对于一些要求变态机动能力的飞行器来说，为了防止俯仰角90°时出现奇点，使用四元数替代欧拉角进行姿态控制是必须的，另外，对于姿态解算时欧拉角描述方法也会出现问题，所以使用四元数进行姿态解算也是上上之策。

欧拉角在俯仰角为±90°时会出现万向锁现象，这是欧拉角表征飞行器姿态的一个局限性。

为了能够清晰得看到欧拉旋转产生的万向锁现象，我们来观察一个三轴的万向节装置——一个陀螺仪：

可以看到中间是一根竖轴穿过一个盘子，而盘子处于高速旋转状态，是陀螺的转子，根据陀螺的定轴性，竖轴也就是陀螺的自转轴在惯性空间内的方向保持不变。

我们先来看看这个陀螺仪的旋转状态。首先解释一下陀螺仪的三个环是怎么旋转的：每一个环都连有一根或者两根轴，这根轴就是每个环的旋转轴。我们可以看到最外面那个环旋转不会影响里面两个环的位置；中间那个环的旋转不会影响最里面那个环的位置，但是最外面的那个环会跟着动；而最里面那个环旋转会影响到外面两个环的位置，这种现象跟我们上面讲的欧拉旋转里的三次旋转是一样的，这也是我们拿陀螺仪来解释欧拉角万向锁现象的原因。

我们先来看一下最外侧那个环的旋转情况，我们上次采用的是ZYX的顺序进行旋转，所以：

最外面绿色环对应的就是绕Z轴的旋转，对应的是飞行器的偏航运动：

中间的红色环对应的就是绕Y轴的旋转，对应的是飞机的俯仰运动：

最里面的蓝色环对应的就是绕X轴的旋转，对应的是飞机的滚转运动：

但是如果飞机俯仰角到达±90°时，会发现此时绿色代表的滚转运动和蓝色代表的偏航运动他们的旋转轴重合了，这时候你必须要改变最里面自转轴的角度才能够达到你需要的空间位置，而这是违背陀螺定轴性规律的。所以最下面的图（滚转）陀螺仪中运动的那个方向其实是被锁住了的，在俯仰角达到±90°时就不可能有这个方向的运动（万向锁时不会有滚转的运动），这是因为当俯仰角达到±90°时，改变了第三个要旋转的轴的方向，它与第一次旋转的Z轴重合了，所以在空间中失去了一个自由度（此时蓝环绕X轴的旋转结果等同于绿环绕Z轴旋转的结果，即都是偏航运动，失去了一个自由度）。

这就是欧拉角的万向锁现象，为了更好的理解这个现象，我们再用自己的手机做一个试验，你把手机屏幕朝上，手机的长边为X轴，短边为Y轴，Z轴垂直屏幕向下，那你先绕Z轴旋转一下手机，假设旋转30度，然后再把手机绕X轴旋转90度。这时候你再绕手机的短边旋转，你会发现手机的长边一直定在桌面上不可能脱离桌面，这就是万向锁现象。

最后，再让我们用数据公式来解释一下万向锁现象产生的原因，我们来回顾一下上面提到的旋转矩阵： \[ R_{b / m v}=R_{\phi} R_{\theta} R_{\psi}=\left[\begin{array}{ccc} \cos \theta \cos \psi & \cos \theta \sin \psi & -\sin \theta \\ \sin \varphi \sin \theta \cos \psi-\cos \varphi \sin \psi & \sin \varphi \sin \theta \sin \psi+\cos \varphi \cos \psi & \sin \varphi \cos \theta \\ \cos \varphi \sin \theta \cos \psi+\sin \varphi \sin \psi & \cos \varphi \sin \theta \sin \psi-\sin \varphi \cos \psi & \cos \varphi \cos \theta \end{array}\right] \] 如果俯仰角为±90°，那么公式就变成了： \[ R_{b / n v}=R_{\phi} R_{\theta} R_{\psi}=\left[\begin{array}{ccc} 0 & 0 & -1 \\ \sin (\varphi-\psi) & \cos (\varphi-\psi) & 0 \\ \cos (\varphi-\psi) & -\sin (\varphi-\psi) & 0 \end{array}\right] \] 这说明改变 \(\phi\) 和 \(\psi\) 的值都是一个效果，而矩阵的第一行和最后一列始终是保持不变，这说明无论你怎么改变 \(\phi\) 和 \(\psi\) ，你的旋转轴一直是Z轴不变，要想改变 \(\phi\) 和 \(\psi\) 有不同的效果，你只能是去改变 \(\theta\) 的值，以上就是用数学方法来解释为什么俯仰角在±90°时欧拉角出现万向锁的过程。

欧拉角表示姿态时会遇到万向锁的问题，这会导致同一种空间状态欧拉角的表示方式不唯一，当出现万向锁现象时，同一种旋转有无数种欧拉角表示形式，从而导致了欧拉角差值时出现问题，因为当你俯仰角接近90°时，两组千差万别的欧拉角表示可以是同一种旋转。所以为了解决这些问题，数学上想出了用四元数的形式来表征姿态的方法。（四元数是由爱尔兰数学家Hamilton发明的，是发明的不是发现的。）

四元数是由1个实数加上3个复数组合而成，通常可以表示成 \(w+xi+yj+zk\) 或者 \((w,(x,y,z))\) ，其中 \(w、x、y、z\) 都是实数，而 \(i^2 = j^2 =k^2 = -1， i^0 = j^0 = k^0 = 1\) 。对于四元数的运算法则，我们要清楚的有以下几个：

假设有两个四元数分别为 \(q1=(w1,(x1,y1,z1))\) 和 \(q2=(w2,(x2,y2,z2))\) ，令 \(v1 = (x1,y1,z1),v2= (x2,y2,z2)\) ，则：

了解了四元数的基本运算规律后，我们来看下它如何表征姿态，假设存在一根旋转轴 \(u\) ，有一个绕 \(u\) 轴旋转 \(\sigma\) 角度的旋转，那这时候代表这个旋转的四元数是： \[ q=\left(\cos \frac{\sigma}{2}, u\mathrm{sin} \frac{\sigma}{2}\right) \] 其中 \(u\) 是旋转轴的单位向量，\(q\) 是一个单位四元数。

上述这个四元数的作用就是，它对任意 \(v\) 向量施加以下算子运算后可以得到该向量绕 \(u\) 轴旋转 \(\sigma\) 角度后的向量 \(w\) ： \[ w = qvq^{*} \] 至于为什么会有这个结果，这儿就不展开证明了，思路就是要证明v和w之间的夹角是 \(\sigma\) 就可以。

我们只需要知道四元数这么写可以用来表征姿态，其实是表征旋转关系，跟旋转矩阵的表示方法类似，只不过它只需要4个元素，而旋转矩阵需要9个元素。